#include <iostream>
#include <iomanip>
#include <chrono>
#include <fstream>
#include <vector>
#include <algorithm>
#include "camera.hpp"
#include "process.hpp"
#include "config.hpp"
#include "console_listener.hpp"

/**
 * 主函数 - 程序入口点
 * 实现了基于OpenCV的实时相机图像处理、形状检测和测距功能
 */
int main() {
    // 输出OpenCV版本信息，便于调试和版本兼容性检查
    std::cout << "OpenCV Version: " << CV_VERSION << std::endl;

    // 从配置文件加载应用程序参数
    AppConfig config;
    loadConfig("config.ini", config);

    // 初始化串口通信（树莓派专用）
    SerialPort serial;
    if (serial.open("/dev/ttyUSB0", 115200)) {
        std::cout << "串口已打开" << std::endl;
        serial.write("System starting...\n");
    } else {
        std::cout << "串口打开失败: " << serial.getLastError() << std::endl;
    }

    // 初始化相机对象并打开相机
    Camera camera(config.camera_index); // 使用配置的相机索引
    if (!camera.open(config)) {
        std::cout << "无法打开相机，请检查相机连接或索引设置" << std::endl;
        return -1;
    }
    camera.start(); // 启动相机采集线程
    
    ConsoleListener listener;
    listener.start(); // 启动控制台监听线程

    // 创建显示窗口
    const std::string winName = config.window_title;
    cv::namedWindow(winName, cv::WINDOW_AUTOSIZE);
    const std::string roi_win_name = "ROI View";
    cv::namedWindow(roi_win_name, cv::WINDOW_AUTOSIZE);

    // 图像帧缓冲区
    cv::Mat frame;         // 原始相机图像
    cv::Mat binary_frame;  // 二值化后的图像，用于形状检测

    // 性能计时器，用于计算和显示帧率
    using clock_t = std::chrono::steady_clock;
    auto t0 = clock_t::now();
    int frameCnt = 0;
    double fps = 0.0;

    // 主处理循环 - 持续处理相机图像帧
    bool should_exit = false;
    while (!should_exit) {
        // 尝试从相机获取新的图像帧
        if (!camera.read(frame)) {
            // 暂时没有新帧，短暂休眠以避免CPU空转
            // 这种情况可能发生在相机帧率较低或处理速度过快时
            std::this_thread::sleep_for(std::chrono::milliseconds(2));
            continue;
        }

        // 处理当前帧，检测外部矩形框架
        // processFrame函数会返回检测到的矩形四个角点，同时生成二值化图像到binary_frame
        auto corners = processFrame(frame, binary_frame, config);

        // 帧计数递增，用于计算帧率
        // 帧计数递增，用于计算帧率
        ++frameCnt;

        // 根据配置，克隆一帧用于显示，以避免在原始图像上绘制
        cv::Mat display_frame = config.show_binary ? binary_frame.clone() : frame.clone();

        // 初始化测量变量
        double pixel_width = 0, pixel_height = 0; // 检测到的物体像素尺寸
        double distance = 0;                      // 计算得到的物体距离
        
        // 在图像上绘制水平参考线，用于辅助标定和观察
        cv::Point pt1(0, 390);                                   // 参考线起点
        cv::Point pt2(display_frame.cols - 1, 390);             // 参考线终点
        cv::line(display_frame, pt1, pt2, cv::Scalar(0, 0, 255), 1); // 绘制红色参考线

        // 检查是否成功检测到外部矩形框架（应有4个角点）
        if (corners && corners->size() == 4) {
            // 计算最小外接旋转矩形，可以处理任意角度的矩形
            cv::RotatedRect bounding_box = cv::minAreaRect(*corners);
            
            // 获取旋转矩形的四个顶点坐标
            cv::Point2f rect_points[4];
            bounding_box.points(rect_points);

            // 计算矩形中心点
            cv::Point2f center_point = (rect_points[0] + rect_points[1] + rect_points[2] + rect_points[3]) * 0.25f;
            
            // 边缘滤波：检查中心点是否在图像边缘30像素范围内
            const int edge_threshold = 30;
            if (center_point.x <= edge_threshold || center_point.x >= display_frame.cols - edge_threshold ||
                center_point.y <= edge_threshold || center_point.y >= display_frame.rows - edge_threshold) {
                continue; // 如果在边缘区域，跳过此矩形
            }
            
            // 计算矩形的像素宽度和高度（用于后续测距计算）
            // 注意：这里假设rect_points[0]-[1]和[1]-[2]分别是相邻的两条边
            pixel_width = cv::norm(rect_points[0] - rect_points[1]);   // 第一条边长度
            pixel_height = cv::norm(rect_points[1] - rect_points[2]);  // 第二条边长度
            
            // -------- 在图像上绘制检测到的矩形及其角点 --------
            for (int i = 0; i < 4; ++i) {
                // 绘制矩形边框（蓝色）
                cv::line(display_frame, rect_points[i], rect_points[(i + 1) % 4], cv::Scalar(255, 0, 0), 2);
                
                // 绘制角点（蓝色实心小点）
                cv::circle(display_frame, rect_points[i], 5, cv::Scalar(255, 0, 0), -1);
            }
            
            // -------- 显示测量结果 --------
            // 显示物体的像素尺寸（宽度和高度）
            std::string size_text = "W: " + std::to_string(static_cast<int>(pixel_width)) + "px, H: " + std::to_string(static_cast<int>(pixel_height)) + "px";
            cv::putText(display_frame, size_text, cv::Point(10, frame.rows - 50), cv::FONT_HERSHEY_SIMPLEX, 0.7, cv::Scalar(255, 255, 0), 2);

            // 准备距离计算结果的字符串流，设置小数点后1位精度
            std::ostringstream dist_oss;
            dist_oss << std::fixed << std::setprecision(1);

            // 基于宽度计算距离（使用针孔相机模型：距离 = 焦距 * 实际尺寸 / 像素尺寸）
            if (pixel_width > 0) {
                // 使用公式: 距离 = (物体实际宽度 * 焦距) / 物体像素宽度
                double dist_w = (config.object_real_width_cm * config.focal_length) / pixel_width;
                distance = dist_w;  // 保存距离值，用于后续内部形状的尺寸计算
                dist_oss << "Dist(W): " << dist_w << "cm";
            }
            
            // 基于高度计算距离（作为参考比较）
            if (pixel_height > 0) {
                // 注意：修正值已删除
                double dist_h = ((config.object_real_height_cm * config.focal_length) / pixel_height);
                if (!dist_oss.str().empty()) dist_oss << ", ";
                dist_oss << "Dist(H): " << dist_h << "cm";
            }
            
            // 在图像底部显示计算得到的距离信息
            cv::putText(display_frame, dist_oss.str(), cv::Point(10, frame.rows - 20), cv::FONT_HERSHEY_SIMPLEX, 0.7, cv::Scalar(0, 255, 255), 2);

            // (调试用) 如果设置了真实距离，则反算并显示实时焦距
            if (config.ground_truth_distance_cm > 0) {
                double f_w = (pixel_width * config.ground_truth_distance_cm) / config.object_real_width_cm;
                double f_h = (pixel_height * config.ground_truth_distance_cm) / config.object_real_height_cm;
                std::ostringstream focal_oss;
                focal_oss << std::fixed << std::setprecision(0) << "Focal(W): " << f_w << ", Focal(H): " << f_h;
                cv::putText(display_frame, focal_oss.str(), cv::Point(10, 20), cv::FONT_HERSHEY_SIMPLEX, 0.6, cv::Scalar(0, 165, 255), 2);
            }
       
            // 优化ROI处理：使用引用而不是克隆，减少内存复制
            cv::Rect outer_frame_roi = bounding_box.boundingRect();
            // 确保 ROI 在图像边界内
            outer_frame_roi &= cv::Rect(0, 0, frame.cols, frame.rows);

            // 稍微缩小 ROI 区域，避免边框自身的像素干扰内部形状检测
            // 使用常量避免重复计算
            const int roi_margin = 10;
            if (outer_frame_roi.width > 2*roi_margin && outer_frame_roi.height > 2*roi_margin) {
                outer_frame_roi.x += roi_margin;
                outer_frame_roi.y += roi_margin;
                outer_frame_roi.width -= 2*roi_margin;
                outer_frame_roi.height -= 2*roi_margin;
            } else {
                // 避免频繁输出警告，只在面积大于一定阈值时输出
                // 继续处理下一帧
                continue;
            }
            
            // 用收缩后的 ROI 检测内部形状 - 使用引用而不是克隆，减少内存复制
            cv::Mat roi = binary_frame(outer_frame_roi);
            cv::imshow(roi_win_name, roi); // 显示ROI窗口
            std::vector<std::vector<cv::Point>> roi_contours;
            // 预分配空间以减少内存重新分配
            roi_contours.reserve(20);
            cv::findContours(roi, roi_contours, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_SIMPLE);

            // 预先计算常量，避免在循环中重复计算
            const double min_area_threshold = config.min_contour_area;
            const double epsilon_factor = config.shape_detection_epsilon;
            const double four_pi = 4 * CV_PI;
            
            for (const auto& c : roi_contours) {
                // 快速过滤小轮廓
                double area = cv::contourArea(c);
                if (area < min_area_threshold) continue;
                
                // 计算周长 - 只计算一次
                double perimeter = cv::arcLength(c, true);
                
                // 计算多边形近似
                std::vector<cv::Point> approx;
                // 预分配空间以减少内存重新分配
                approx.reserve(8); // 通常不会超过8个顶点
                cv::approxPolyDP(c, approx, epsilon_factor * perimeter, true);
                
                // 计算圆度 - 使用预计算的常量
                double circularity = four_pi * area / (perimeter * perimeter);

                // 计算质心 - 只在需要时计算
                cv::Moments m = cv::moments(c);
                // 避免除以零
                if (m.m00 < 1e-5) continue;
                cv::Point center(static_cast<int>(m.m10/m.m00) + outer_frame_roi.x, 
                                static_cast<int>(m.m01/m.m00) + outer_frame_roi.y);
                
                // 添加调试输出，显示形状的圆度、顶点数和面积
                // 只显示面积大于一定值的形状，避免输出过多
                if (area > 5000) {
                    std::cout << "形状检测 - 圆度: " << circularity << ", 顶点数: " << approx.size() 
                              << ", 面积: " << area << std::endl;
                }

                // 三角形检测：允许顶点数为3或接近3的形状
                // 在远距离时，由于图像质量下降，可能无法精确检测到三个顶点
                if (approx.size() == 3 || (approx.size() >= 3 && approx.size() < 5 && circularity < 0.6)) {
                    // 三角形：取两个相邻角点
                    cv::Point pt1 = approx[0] + cv::Point(outer_frame_roi.x, outer_frame_roi.y);
                    cv::Point pt2 = approx[1] + cv::Point(outer_frame_roi.x, outer_frame_roi.y);
                    double pixel_side = cv::norm(pt1 - pt2);
                    double real_side = (pixel_side * distance) / config.focal_length;
                    
                    // 降低三角形检测的阈值，使远距离时更容易检测到
                    cv::line(display_frame, pt1, pt2, cv::Scalar(0,255,0), 2); // 标出边
                    cv::drawContours(display_frame, std::vector<std::vector<cv::Point>>{approx}, -1, cv::Scalar(255,0,0), 2, cv::LINE_8, {}, 0, outer_frame_roi.tl());
                    cv::putText(display_frame, "Triangle: " + std::to_string(real_side) + "cm", center, cv::FONT_HERSHEY_SIMPLEX, 0.7, cv::Scalar(255,0,0), 2);
                } else if (approx.size() == 4 || (approx.size() > 4 && approx.size() <= 6 && circularity > 0.6 && circularity < 0.65)) {
                    // 正方形/矩形：检查长宽比和四边长度
                    // 在远距离时，允许顶点数有一定的误差
                    
                    // 计算四边长度 - 优化：预分配空间并直接计算方差
                    const size_t n_sides = approx.size();
                    std::vector<double> sides(n_sides);
                    double total_length = 0.0;
                    
                    // 第一次遍历：计算边长和总长度
                    for (size_t i = 0; i < n_sides; i++) {
                        cv::Point pt1 = approx[i] + cv::Point(outer_frame_roi.x, outer_frame_roi.y);
                        cv::Point pt2 = approx[(i + 1) % n_sides] + cv::Point(outer_frame_roi.x, outer_frame_roi.y);
                        sides[i] = cv::norm(pt1 - pt2);
                        total_length += sides[i];
                    }
                    
                    // 计算边长方差，判断四边是否接近等长
                    const double mean_side = total_length / n_sides;
                    double variance = 0.0;
                    
                    // 第二次遍历：计算方差 - 使用快速算法
                    for (size_t i = 0; i < n_sides; i++) {
                        double diff = sides[i] - mean_side;
                        variance += diff * diff;
                    }
                    variance /= n_sides;
                    double normalized_variance = variance / (mean_side * mean_side); // 归一化方差
                    
                    // 计算最小外接矩形，用于判断长宽比
                    cv::RotatedRect min_rect = cv::minAreaRect(approx);
                    double width = min_rect.size.width;
                    double height = min_rect.size.height;
                    double aspect_ratio = (width < height) ? width / height : height / width; // 确保比例在0-1之间
                    double aspect_ratio_diff = 1.0 - aspect_ratio; // 与1的差距
                    
                    // 输出调试信息
                    if (area > 5000) {
                        std::cout << "正方形检测 - 长宽比: " << aspect_ratio << ", 边长方差: " << normalized_variance 
                                  << ", 顶点数: " << approx.size() << std::endl;
                    }
                    
                    // 判断是否为正方形：长宽比接近1，四边长度接近相等
                    bool is_square = aspect_ratio_diff <= config.square_aspect_ratio_threshold && 
                                     normalized_variance <= config.square_side_variance_threshold;
                    
                    // 计算边长和实际尺寸
                    // 使用平均边长而不是单一边长，提高测量稳定性
                    double pixel_side = mean_side;
                    double real_side = (pixel_side * distance) / config.focal_length;
                    
                    // 绘制所有边，而不仅仅是一条边
                    for (int i = 0; i < approx.size(); i++) {
                        cv::Point pt1 = approx[i] + cv::Point(outer_frame_roi.x, outer_frame_roi.y);
                        cv::Point pt2 = approx[(i + 1) % approx.size()] + cv::Point(outer_frame_roi.x, outer_frame_roi.y);
                        cv::line(display_frame, pt1, pt2, cv::Scalar(0,255,0), 2);
                    }
                    
                    cv::drawContours(display_frame, std::vector<std::vector<cv::Point>>{approx}, -1, cv::Scalar(0,255,255), 2, cv::LINE_8, {}, 0, outer_frame_roi.tl());
                    
                    // 显示形状类型和尺寸
                    std::string shape_name = is_square ? "Square" : "Rectangle";
                    cv::putText(display_frame, shape_name + ": " + std::to_string(real_side) + "cm", 
                                center, cv::FONT_HERSHEY_SIMPLEX, 0.7, cv::Scalar(0,255,255), 2);
                    
                    // 显示长宽比和方差信息
                    std::ostringstream info_text;
                    info_text << std::fixed << std::setprecision(2) 
                              << "AR:" << aspect_ratio << " Var:" << normalized_variance;
                    cv::putText(display_frame, info_text.str(), 
                                cv::Point(center.x, center.y + 20), 
                                cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(0,255,255), 1);
                } else if((approx.size() > 6 && circularity > 0.65) || (circularity > 0.8)){
                    // 圆形：找出轮廓上距离最远的两个点作为直径端点
                    // 在远距离时，放宽圆形检测条件，主要依靠circularity判断
                    
                    // 优化：使用旋转矩形的长轴作为直径，避免O(n²)的双重循环
                    cv::RotatedRect min_rect = cv::minAreaRect(c);
                    float width = min_rect.size.width;
                    float height = min_rect.size.height;
                    float max_dist = std::max(width, height);
                    
                    // 获取旋转矩形的四个顶点
                    cv::Point2f rect_points[4];
                    min_rect.points(rect_points);
                    
                    // 找出长轴的两个端点
                    cv::Point pt1, pt2;
                    if (width > height) {
                        pt1 = cv::Point(rect_points[0]);
                        pt2 = cv::Point(rect_points[1]);
                    } else {
                        pt1 = cv::Point(rect_points[1]);
                        pt2 = cv::Point(rect_points[2]);
                    }
                    pt1 += cv::Point(outer_frame_roi.x, outer_frame_roi.y);
                    pt2 += cv::Point(outer_frame_roi.x, outer_frame_roi.y);
                    double pixel_diameter = cv::norm(pt1 - pt2);
                    double real_diameter = (pixel_diameter * distance) / config.focal_length;
                    cv::line(display_frame, pt1, pt2, cv::Scalar(0,255,0), 2); // 标出直径
                    cv::circle(display_frame, center, pixel_diameter/2, cv::Scalar(0,0,255), 2);
                    cv::putText(display_frame, "Circle: " + std::to_string(real_diameter) + "cm", center, cv::FONT_HERSHEY_SIMPLEX, 0.7, cv::Scalar(0,0,255), 2);
                }  
            }
        }

        // 计算并显示帧率
        auto now = clock_t::now();
        double elapsedSec = std::chrono::duration<double>(now - t0).count();
        if (elapsedSec > 0.0) {
            fps = frameCnt / elapsedSec;
        }

        std::ostringstream oss;
        oss << std::fixed << std::setprecision(1)
            << winName << "  |  FPS: " << fps
            << "  |  Time: " << std::setprecision(0) << elapsedSec << "s";
        cv::setWindowTitle(winName, oss.str());

        // 在显示前，根据配置缩放图像
        cv::Mat final_display;
        if (config.display_scale_factor > 0 && config.display_scale_factor != 1.0) {
            cv::resize(display_frame, final_display, cv::Size(),
                       config.display_scale_factor, config.display_scale_factor,
                       cv::INTER_AREA);
        } else {
            final_display = display_frame;
        }
        cv::imshow(winName, final_display);

        int key = cv::waitKey(1);
        if (key == 27) { // ESC 键
            std::cout << "检测到'ESC'键按下，正在退出程序..." << std::endl;
            break;
        } else if (key == 'p' || key == 'P') { // P 键
            // 生成带时间戳的文件名
            auto now = std::chrono::system_clock::now();
            auto in_time_t = std::chrono::system_clock::to_time_t(now);
            std::stringstream ss;
            ss << "snapshot_" << std::put_time(std::localtime(&in_time_t), "%Y%m%d_%H%M%S") << ".png";
            std::string filename = ss.str();

            // 保存原始、无任何绘制的图像帧
            cv::imwrite(filename, frame);
            std::cout << "照片已保存: " << filename << std::endl;
        }
    }

    cv::destroyAllWindows();
    // camera.stop() 会在 camera 对象销毁时由析构函数自动调用
    serial.close();  // 关闭串口
    return 0;
}